
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_accel_cal.c
  * @author     baiyang
  * @date       2022-3-13
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_accel_cal.h"

#include <rtthread.h>

#include "sensor_imu.h"

#include <common/time/gp_time.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/
#define AP_ACCELCAL_POSITION_REQUEST_INTERVAL_MS 1000

#define _printf(format, args...) mavproxy_send_statustext(MAV_SEVERITY_CRITICAL, format, ##args)
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void accel_cal_update_status(accel_cal_t acc_cal);
static void accel_cal_fail(accel_cal_t acc_cal);
static void accel_cal_clear(accel_cal_t acc_cal);
static void accel_cal_collect_sample(accel_cal_t acc_cal);
static bool accel_cal_client_active();
static void accel_cal_success(accel_cal_t acc_cal);
/*----------------------------------variable----------------------------------*/
static bool _start_collect_sample;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]   saccelcal  
  * @param[out]  
  * @retval      
  * @note        
  */
void accel_cal_ctor(accel_cal_t acc_cal)
{
    rt_memset(acc_cal, 0, sizeof(struct accel_cal));

    acc_cal->_use_gcs_snoop = true;
    acc_cal->_started = false;
    acc_cal->_saving = false;
    acc_cal->_waiting_for_mavlink_ack = false;

    accel_cal_update_status(acc_cal);
}

/**
  * @brief       
  * @param[in]   acc_cal  
  * @param[out]  
  * @retval      
  * @note        
  */
void accel_cal_update(accel_cal_t acc_cal)
{
    if (!accel_cal_get_calibrator(0)) {
        // no calibrators
        return;
    }

    if (acc_cal->_started) {
        accel_cal_update_status(acc_cal);

        accelcal_t cal;
        uint8_t num_active_calibrators = 0;
        for(uint8_t i=0; (cal = accel_cal_get_calibrator(i)); i++) {
            num_active_calibrators++;
        }
        if (num_active_calibrators != acc_cal->_num_active_calibrators) {
            accel_cal_fail(acc_cal);
            return;
        }
        if(_start_collect_sample) {
            accel_cal_collect_sample(acc_cal);
        }
        switch(acc_cal->_status) {
            case ACCEL_CAL_NOT_STARTED:
                accel_cal_fail(acc_cal);
                return;
            case ACCEL_CAL_WAITING_FOR_ORIENTATION: {
                // if we're waiting for orientation, first ensure that all calibrators are on the same step
                uint8_t step;
                if ((cal = accel_cal_get_calibrator(0)) == NULL) {
                    accel_cal_fail(acc_cal);
                    return;
                }
                step = accelcal_get_num_samples_collected(cal)+1;

                for(uint8_t i=1 ; (cal = accel_cal_get_calibrator(i))  ; i++) {
                    if (step != accelcal_get_num_samples_collected(cal)+1) {
                        accel_cal_fail(acc_cal);
                        return;
                    }
                }
                // if we're on a new step, print a message describing the step
                if (step != acc_cal->_step) {
                    acc_cal->_step = step;

                    if(acc_cal->_use_gcs_snoop) {
                        const char *msg;
                        switch (step) {
                            case ACCELCAL_VEHICLE_POS_LEVEL:
                                msg = "level";
                                break;
                            case ACCELCAL_VEHICLE_POS_LEFT:
                                msg = "on its LEFT side";
                                break;
                            case ACCELCAL_VEHICLE_POS_RIGHT:
                                msg = "on its RIGHT side";
                                break;
                            case ACCELCAL_VEHICLE_POS_NOSEDOWN:
                                msg = "nose DOWN";
                                break;
                            case ACCELCAL_VEHICLE_POS_NOSEUP:
                                msg = "nose UP";
                                break;
                            case ACCELCAL_VEHICLE_POS_BACK:
                                msg = "on its BACK";
                                break;
                            default:
                                accel_cal_fail(acc_cal);
                                return;
                        }
                        _printf("Place vehicle %s and press any key.", msg);
                        acc_cal->_waiting_for_mavlink_ack = true;
                    }
                }

                uint32_t now = time_millis();
                if (now - acc_cal->_last_position_request_ms > AP_ACCELCAL_POSITION_REQUEST_INTERVAL_MS) {
                    acc_cal->_last_position_request_ms = now;
                    mavproxy_send_accelcal_vehicle_position(step);
                }
                break;
            }
            case ACCEL_CAL_COLLECTING_SAMPLE:
                // check for timeout

                for(uint8_t i=0; (cal = accel_cal_get_calibrator(i)); i++) {
                    accelcal_check_for_timeout(cal);
                }

                accel_cal_update_status(acc_cal);

                if (acc_cal->_status == ACCEL_CAL_FAILED) {
                    accel_cal_fail(acc_cal);
                }
                return;
            case ACCEL_CAL_SUCCESS:
                // save
                if (acc_cal->_saving) {
                    bool done = true;
                    if (accel_cal_client_active() && sensor_imu_acal_get_saving()) {
                        done = false;
                    }
                    if (done) {
                        accel_cal_success(acc_cal);
                    }
                    return;
                } else {
                    if (accel_cal_client_active() && sensor_imu_acal_get_fail()) {
                        accel_cal_fail(acc_cal);
                    }

                    if (accel_cal_client_active()) {
                        sensor_imu_acal_save_calibrations();
                    }
                    acc_cal->_saving = true;
                }
                return;
            default:
            case ACCEL_CAL_FAILED:
                accel_cal_fail(acc_cal);
                return;
        }
    } else if (acc_cal->_last_result != ACCEL_CAL_NOT_STARTED) {
        // only continuously report if we have ever completed a calibration
        uint32_t now = time_millis();
        if (now - acc_cal->_last_position_request_ms > AP_ACCELCAL_POSITION_REQUEST_INTERVAL_MS) {
            acc_cal->_last_position_request_ms = now;
            switch (acc_cal->_last_result) {
                case ACCEL_CAL_SUCCESS:
                    mavproxy_send_accelcal_vehicle_position(ACCELCAL_VEHICLE_POS_SUCCESS);
                    break;
                case ACCEL_CAL_FAILED:
                    mavproxy_send_accelcal_vehicle_position(ACCELCAL_VEHICLE_POS_FAILED);
                    break;
                default:
                    // should never hit this state
                    break;
            }
        }
    }

}

void accel_cal_start(accel_cal_t acc_cal)
{
    if (acc_cal->_started) {
        return;
    }
    _start_collect_sample = false;
    acc_cal->_num_active_calibrators = 0;

    accelcal_t cal;
    for(uint8_t i=0; (cal = accel_cal_get_calibrator(i)); i++) {
        accelcal_clear(cal);
        accelcal_start(cal, ACCEL_CAL_AXIS_ALIGNED_ELLIPSOID, 6, 0.5f);
        acc_cal->_num_active_calibrators++;
    }

    acc_cal->_started = true;
    acc_cal->_saving = false;
    acc_cal->_use_gcs_snoop = true;
    acc_cal->_last_position_request_ms = 0;
    acc_cal->_step = 0;

    acc_cal->_last_result = ACCEL_CAL_NOT_STARTED;

    accel_cal_update_status(acc_cal);
}

/**
  * @brief       
  * @param[in]   acc_cal  
  * @param[out]  
  * @retval      
  * @note        
  */
void accel_cal_cancel(accel_cal_t acc_cal)
{
    _printf("Calibration cancelled");

    acc_cal->_last_result = ACCEL_CAL_NOT_STARTED;

    accel_cal_clear(acc_cal);
}

// true if we are in a calibration process
bool accel_cal_running(accel_cal_t acc_cal)
{
    return acc_cal->_status == ACCEL_CAL_WAITING_FOR_ORIENTATION || acc_cal->_status == ACCEL_CAL_COLLECTING_SAMPLE;
}

accelcal_t accel_cal_get_calibrator(uint8_t index) {
    accelcal_t ret;
    for(uint8_t j=0 ; (ret = sensor_imu_acal_get_calibrator(j)) ; j++) {
        if (index == 0) {
            return ret;
        }
        index--;
    }
    return NULL;
}

void accel_cal_handleMessage(accel_cal_t acc_cal, const mavlink_message_t *msg)
{
    if (!acc_cal->_waiting_for_mavlink_ack) {
        return;
    }
    acc_cal->_waiting_for_mavlink_ack = false;
    if (msg->msgid == MAVLINK_MSG_ID_COMMAND_ACK) {
        _start_collect_sample = true;
    }
}

bool accel_cal_gcs_vehicle_position(accel_cal_t acc_cal, float position)
{
    acc_cal->_use_gcs_snoop = false;

    if (acc_cal->_status == ACCEL_CAL_WAITING_FOR_ORIENTATION && math_flt_equal((float) acc_cal->_step, position)) {
        _start_collect_sample = true;
        return true;
    }

    return false;
}

static void accel_cal_update_status(accel_cal_t acc_cal)
{
    accelcal_t cal;

    if (!accel_cal_get_calibrator(0)) {
        // no calibrators
        acc_cal->_status = ACCEL_CAL_NOT_STARTED;
        return;
    }

    for(uint8_t i=0 ; (cal = accel_cal_get_calibrator(i)); i++) {
        if (accelcal_get_status(cal) == ACCEL_CAL_FAILED) {
            acc_cal->_status = ACCEL_CAL_FAILED;         //fail if even one of the calibration has
            return;
        }
    }

    for(uint8_t i=0 ; (cal = accel_cal_get_calibrator(i)); i++) {
        if (accelcal_get_status(cal) == ACCEL_CAL_COLLECTING_SAMPLE) {
            acc_cal->_status = ACCEL_CAL_COLLECTING_SAMPLE;          // move to Collecting sample state if all the callibrators have
            return;
        }
    }

    for(uint8_t i=0 ; (cal = accel_cal_get_calibrator(i)); i++) {
        if (accelcal_get_status(cal) == ACCEL_CAL_WAITING_FOR_ORIENTATION) {
            acc_cal->_status = ACCEL_CAL_WAITING_FOR_ORIENTATION;    // move to waiting for user ack for orientation confirmation
            return;
        }
    }

    for(uint8_t i=0 ; (cal = accel_cal_get_calibrator(i)); i++) {
        if (accelcal_get_status(cal) == ACCEL_CAL_NOT_STARTED) {
            acc_cal->_status = ACCEL_CAL_NOT_STARTED;    // we haven't started if all the calibrators haven't
            return;
        }
    }

    acc_cal->_status = ACCEL_CAL_SUCCESS;    // we have succeeded calibration if all the calibrators have
    return;
}

static void accel_cal_fail(accel_cal_t acc_cal)
{
    _printf("Calibration FAILED");

    sensor_imu_acal_event_failure();

    acc_cal->_last_result = ACCEL_CAL_FAILED;

    accel_cal_clear(acc_cal);
}

static void accel_cal_clear(accel_cal_t acc_cal)
{
    if (!acc_cal->_started) {
        return;
    }

    accelcal_t cal;
    for(uint8_t i=0 ; (cal = accel_cal_get_calibrator(i))  ; i++) {
        accelcal_clear(cal);
    }

    acc_cal->_step = 0;
    acc_cal->_started = false;
    acc_cal->_saving = false;

    accel_cal_update_status(acc_cal);
}

static void accel_cal_collect_sample(accel_cal_t acc_cal)
{
    if (acc_cal->_status != ACCEL_CAL_WAITING_FOR_ORIENTATION) {
        return;
    }

    if (accel_cal_client_active() && !sensor_imu_acal_get_ready_to_sample()) {
        _printf("Not ready to sample");
        return;
    }

    accelcal_t cal;
    for(uint8_t i=0 ; (cal = accel_cal_get_calibrator(i))  ; i++) {
        accelcal_collect_sample(cal);
    }
    _start_collect_sample = false;
    accel_cal_update_status(acc_cal);
}

static bool accel_cal_client_active()
{
    return (bool)sensor_imu_acal_get_calibrator(0);
}

static void accel_cal_success(accel_cal_t acc_cal)
{
    _printf("Calibration successful");
    acc_cal->_last_result = ACCEL_CAL_SUCCESS;
    accel_cal_clear(acc_cal);
}
/*------------------------------------test------------------------------------*/


